//---------------------------------------------------------------------------

#ifndef MotorH
#define MotorH

#define INSTRUCTION_MODE	 	0

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
typedef struct
{
	unsigned char Buff[MOTOR_RX_BUFF_SIZE];
	int NowIndex;
	int EndIndex;
} MotorRXStruct;
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
typedef enum
{
	HEAD1											= 0,
	HEAD2											= 1,
	ID_SYNCWRITE							= 2,
	TOTAL_LENGTH							= 3,
	INSTRUCTION_SYNCWRITE			= 4,
	FIRST_ADDRESS							= 5,
	SINGLE_DATA_LENGTH				= 6,
	PACKET_HEAD_LENGTH				= 7
} MotorDataPacketHeadDef;

typedef enum
{
	ID                	= 0,
	LOW_POSITION 				= 1,
	HIGH_POSITION 			= 2,
	LOW_SPEED						= 3,
	HIGH_SPEED					= 4,
	MOTOR_WRITE_LENGTH	= 5
} MotorDataWriteDef;

typedef enum
{
	READ_ID							= 0,			// or IMU  or  FSR
	READ_LOW_POSITION		= 1,			// X axis  low_byte data
	READ_HIGH_POSITION	= 2,			// X axis hign_byte data
	READ_LOW_LOADING		= 3,			// Y axis  low_byte data
	READ_HIGH_LOADING 	= 4,		  // Y axis hign_byte data
	READ_VOLTAGE				= 5,			// Z axis  low_byte data
	READ_RESERVE        = 6,			// Z axis hign_byte data
	READ_CHECKSUM				= 7,
	MOTOR_READ_LENGTH		= 8
} MotorDataReadDef;

typedef enum
{
	TORQUE_ID                	= 0,
	TORQUE_ENABLED		 				= 1,
	MOTOR_TORQUE_LENGTH				= 2
} MotorTorqueWriteDef;

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
// Definition of Motor Data
typedef struct
{
	unsigned char PacketHead_UB[PACKET_HEAD_LENGTH];
	unsigned char Write_UB[MOTOR_NUM_UB][MOTOR_WRITE_LENGTH];
	unsigned char Checksum_UB;

	unsigned char PacketHead_RL[PACKET_HEAD_LENGTH];
	unsigned char Write_RL[MOTOR_NUM_RL][MOTOR_WRITE_LENGTH];
	unsigned char Checksum_RL;

	unsigned char PacketHead_LL[PACKET_HEAD_LENGTH];
	unsigned char Write_LL[MOTOR_NUM_LL][MOTOR_WRITE_LENGTH];
	unsigned char Checksum_LL;

	unsigned short WtiteLength_UB;
	unsigned short WtiteLength_RL;
	unsigned short WtiteLength_LL;

	unsigned char Checksum_PacketHead[BODYPART_NUM];
	//////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////
	unsigned char Read[MOTOR_READ_TOTAL_NUM][MOTOR_READ_LENGTH];
	unsigned char Battery[BODYPART_NUM];
	unsigned char BatteryCount[BODYPART_NUM];
} MotorDataStruct;

typedef struct
{
	unsigned char PacketHead_UB[PACKET_HEAD_LENGTH];
	unsigned char Write_UB[MOTOR_NUM_UB][MOTOR_TORQUE_LENGTH];
	unsigned char Checksum_UB;

	unsigned char PacketHead_RL[PACKET_HEAD_LENGTH];
	unsigned char Write_RL[MOTOR_NUM_RL][MOTOR_TORQUE_LENGTH];
	unsigned char Checksum_RL;

	unsigned char PacketHead_LL[PACKET_HEAD_LENGTH];
	unsigned char Write_LL[MOTOR_NUM_LL][MOTOR_TORQUE_LENGTH];
	unsigned char Checksum_LL;

	unsigned char Checksum_PacketHead[BODYPART_NUM];

	unsigned short WtiteLength_UB;
	unsigned short WtiteLength_RL;
	unsigned short WtiteLength_LL;
} MotortTorqueStruct;

typedef struct
{
	unsigned char *PacketHead[BODYPART_NUM];
	unsigned char *Write[BODYPART_NUM];
	unsigned char *Checksum[BODYPART_NUM];
	unsigned short *WtiteLength[BODYPART_NUM];
}MotorWriteAddressStruct;

typedef struct
{
	unsigned char *PacketHead[BODYPART_NUM];
	unsigned char *Write[BODYPART_NUM];
	unsigned char *Checksum[BODYPART_NUM];
	unsigned short *WtiteLength[BODYPART_NUM];
}MotorTorqueAddressStruct;

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

// Definition of Motor Schedule
typedef struct {
	bool Enabled;
	int NowIndex;
	int EndIndex;
	unsigned char List[MOTOR_SCHEDULE_SIZE][MOTOR_SCHEDULE_LIST_LENGTH];
	unsigned int Delay_ExecutedCount; // Count of executing the function Update_MotorSchedule_to_MotorData()
	WorkStatus TorqueChanged;
} MotorScheduleStruct;

// Definition of Head Schedule
typedef struct {
	int NowIndex;
	int EndIndex;
	unsigned char List[HEAD_SCHEDULE_SIZE][HEAD_SCHEDULE_LIST_LENGTH];
	unsigned int Delay_ExecutedCount; // Count of executing the function Update_MotorSchedule_to_MotorData()
} HeadScheduleStruct;

// Definition of Motor Schedule List
typedef enum {
	INSERT_DELAY_COUNT	= 255,			// Used for dividing motion into poses as the following example.
	SET_STEP_TIME				= 253,
	SET_POSITION	  		= MOTOR_NUM,										// instruction byte < SET_POSITION	-> id = instruction byte
	SET_TORQUE	 				= MOTOR_MAX_ID + TORQUE_BASE		// instruction byte < SET_TORQUE		-> id = instruction byte - TORQUE_BASE
}	MotorScheduleInstruction;
/*	Example for MotorScheduleInstruction:
		Add_motor_job(  253,  10,   0,   0,   0 ); // Set step time as 10*10 ms
		Add_motor_job(   31, 196,   7,  56,   0 ); // Pose 1 for Motor 31
		Add_motor_job(   33,  40,   8,  37,   0 ); // Pose 1 for Motor 33
		Add_motor_job(   34, 106,   7, 141,   0 ); // Pose 1 for Motor 34
		Add_motor_job(  255,   2,   0,   0,   0 );
		Add_motor_job(   31, 250,   6, 127,   0 ); // Pose 2 for Motor 31
		Add_motor_job(   32, 190,   8, 119,   0 ); // Pose 2 for Motor 32
		Add_motor_job(   33,  22,   6,  77,   1 ); // Pose 2 for Motor 33
		Add_motor_job(  255,   3,   0,   0,   0 );
*/
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

extern MotorRXStruct MotorRX[BODYPART_NUM];
extern MotorDataStruct MotorData;
extern MotorWriteAddressStruct MotorAddress;
extern MotortTorqueStruct MotorTorque;
extern MotorWriteAddressStruct MotorTorqueAddress;
extern MotorScheduleStruct MotorSchedule;
extern HeadScheduleStruct HeadSchedule;

extern bool MotorWritten;
extern bool HeadWritten;
extern bool MotorWrittenFinish;

extern int TimerInterval_WriteMotor;
extern int TimerInterval_ReadMotor;

void Motor_Init(void);
void MotorRX_Init(void);
void MotorAddress_Init(void);
void MotorData_Init(void);
void MotorSchedule_Init(void);
void HeadSchedule_Init(void);

void CalculateChecksum(void);
void CalculateReadChecksum(void);
void WriteMotor(BodyPartStatus part);
void WriteTorque(BodyPartStatus part);

bool ReadMotor(unsigned char id_index);
void CheckMotorRX(BodyPartStatus bodypart);

void MotorScheduleSwitch(bool enabled);
void Update_MotorSchedule_to_MotorData(void);
void Update_HeadSchedule_to_MotorData(void);

void Add_motor_job(unsigned char data0, unsigned char data1, unsigned char data2, unsigned char data3, unsigned char data4);
void Add_head_job(unsigned char data0, unsigned char data1, unsigned char data2, unsigned char data3, unsigned char data4);
bool IDtoPartNo(unsigned char id, int *part, int *no);
int ID2Index(unsigned char id);

void ShowMotorSchedule(TMemo *memo_show);
void ShowHeadSchedule(TMemo *memo_show);
void ShowMotorWriteData(TMemo *memo_show);
void ShowMotorReadData(TMemo *memo_show);
void ShowMotorRX(TMemo *memo_show);
//---------------------------------------------------------------------------
#endif
